﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Mahanet.Abyss.PathCalculation
{
	public class GraviticLeastSquaresCoordinate2DPathOracle : IPathOracle<double, Coordinate2D>
	{
		#region Fields

		private List<DataPoint<double, Coordinate2D>> m_DataPoints = new List<DataPoint<double, Coordinate2D>>();
		private alglib.polynomialfitreport m_LastReport;

		#endregion

		#region Public Properties

		public FunctionApproximation<double, Coordinate2D> Approximation { get; private set; }

		public Coordinate2D GeneratedXPoint { get; private set; }

		public Coordinate2D GeneratedYPoint { get; private set; }

		public int DataPointCount
		{
			get { return m_DataPoints.Count; }
		}

		#endregion

		#region Public Methods

		public void AddPointData(DataPoint<double, Coordinate2D> point)
		{
			m_DataPoints.Add(point);
			Approximation = null;
		}

		public void GenerateApproximation()
		{
			int info = 0;
			List<double> tValues = m_DataPoints.Select(dataPoint => dataPoint.Paramter).ToList();
			List<double> xValues = m_DataPoints.Select(dataPoint => dataPoint.Value.X).ToList();
			List<double> yValues = m_DataPoints.Select(dataPoint => dataPoint.Value.Y).ToList();
			List<double> weights = Enumerable.Repeat(1, m_DataPoints.Count).Select(i => (double)i).ToList();

			AddGraviticDataPoint(tValues, xValues, yValues, weights);

			alglib.barycentricinterpolant xApproximation;
			alglib.barycentricinterpolant yApproximation;
			alglib.polynomialfitwc(tValues.ToArray(), xValues.ToArray(), weights.ToArray(), new double[] { }, new double[] { }, new int[] { }, 3, out info, out xApproximation, out m_LastReport);
			alglib.polynomialfitwc(tValues.ToArray(), yValues.ToArray(), weights.ToArray(), new double[] { }, new double[] { }, new int[] { }, 3, out info, out yApproximation, out m_LastReport);

			Approximation = t =>
			{
				var x = alglib.barycentriccalc(xApproximation, t);
				var y = alglib.barycentriccalc(yApproximation, t);

				return new Coordinate2D(x, y);
			};
		}

		public double GetCertainty()
		{
			return m_DataPoints.Count == 0 ? 0 : 1.0 - (1.8 / (double)(m_DataPoints.Count + 1));
		}

		public string GetLastReportString()
		{
			return string.Format("On points: Max error {0}, avg error {1}, avg rel error {2}", m_LastReport.maxerror, m_LastReport.avgerror, m_LastReport.avgrelerror);
		}

		#endregion

		#region Private Methods

		private void AddGraviticDataPoint(List<double> tValues, List<double> xValues, List<double> yValues, List<double> weights)
		{
			if (tValues.Count < 3)
			{
				return;
			}

			double gravity = Coordinate2DDataGenerator.APPROXIMATE_GRAVITY;

			int pa = yValues.Count - 2;
			int pb = yValues.Count - 1;

			Coordinate2D pointy1 = new Coordinate2D(tValues.ElementAt(pa), yValues.ElementAt(pa));
			Coordinate2D pointy2 = new Coordinate2D(tValues.ElementAt(pb), yValues.ElementAt(pb));
			Coordinate2D pointx1 = new Coordinate2D(tValues.ElementAt(pa), xValues.ElementAt(pa));
			Coordinate2D pointx2 = new Coordinate2D(tValues.ElementAt(pb), xValues.ElementAt(pb));

			GeneratedYPoint = CalculateNextPoint(gravity, pointy1, pointy2);
			GeneratedXPoint = CalculateNextPoint(0, pointx1, pointx2);

			tValues.Add(GeneratedYPoint.X);
			yValues.Add(GeneratedYPoint.Y);
			xValues.Add(GeneratedXPoint.Y);
			weights.Add(2);
		}

		private Coordinate2D CalculateNextPoint(double gravity, Coordinate2D point1, Coordinate2D point2)
		{
			double xDifference = (point2.X - point1.X);
			
			double b = ((point2.Y - gravity * point2.X * point2.X) - (point1.Y - gravity * point1.X * point1.X)) /
						xDifference;

			double c = point1.Y - gravity * point1.X * point1.X - b * point1.X;

			double newX = point2.X + xDifference;
			double newY = gravity * newX * newX + b * newX + c;

			return new Coordinate2D(newX, newY);
		}

		#endregion
	}
}
